﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using TrackingSim.Filtering;
using TrackingSim.GraphicSystem;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
using TrackingSim.Filtering.Measurements;
using TrackingSim.Filtering.Updating;
using TrackingSim.Filtering.Tracks;
using TrackingSim.Filtering.Kinematics;

namespace TrackingSim.GameSystem
{
    class Tracker
    {
        Player host;
        private Dictionary<int,VertexBuffer> track_vertex_buffers;
        private Dictionary<int, VertexBuffer> track_measurement_vertex_buffers;
        Dictionary<int, Track> track_map;

        public Tracker(Player _host)
        {
            host = _host;
            track_vertex_buffers = new Dictionary<int, VertexBuffer>();
            track_measurement_vertex_buffers = new Dictionary<int, VertexBuffer>();
            track_map = new Dictionary<int, Track>();
        }

        public void update(Player p, Measurement meas)
        {
            if (!track_map.ContainsKey(p.PlayerNumber))
            {
                Track t = TrackCreator.create_track(meas);
                t.init(host, meas);

                track_map.Add(p.PlayerNumber, t);
            }
            else
            {
                Track track = track_map[p.PlayerNumber];

                meas.accept(track, host);

                //track.update(meas, host);
            }
        }

        public void draw_tracks(GraphicsDevice device)
        {
            var track_map_enumer = track_map.GetEnumerator();

            while (track_map_enumer.MoveNext())
            {
                int track_number = track_map_enumer.Current.Key;

                Track track = track_map[track_number];

                int vertex_count = 0;
                VertexPositionNormalTexture[] ellipse_vertices = Drawing.get_ellipse_vertices(track, 10, ref vertex_count);

                if (!track_vertex_buffers.ContainsKey(track_number))
                {
                    track_vertex_buffers.Add(track_number, new VertexBuffer(device, VertexPositionNormalTexture.VertexDeclaration, vertex_count, BufferUsage.WriteOnly));
                }

                track_vertex_buffers[track_number].SetData<VertexPositionNormalTexture>(ellipse_vertices);
                device.SetVertexBuffer(track_vertex_buffers[track_number]);
                device.DrawPrimitives(PrimitiveType.LineList, 0, ellipse_vertices.Length / 2);
            }
        }

        public void print_data()
        {
            var enumer = track_map.GetEnumerator();

            while (enumer.MoveNext())
            {
                StreamWriter sw = File.AppendText("Tracker_data_pl_" + enumer.Current.Key + ".csv");
                sw.WriteLine("Time,PosX,PosY,PosZ,Est Rng,True PosX,True PosY,True PosZ,True Rng,Meas Rng,Err Rng");

                //foreach (Track track in enumer.Current.Value)
                {
                    //double est_range = track.State.pos_mag();
                    //double true_range = track.MeasTrueRcycz.TruePos.mag();
                    //double error_range = est_range - true_range;

                    //sw.WriteLine( track.Time + "," +
                    //              track.State.X + "," +
                    //              track.State.Y + "," +
                    //              track.State.Z + "," +
                    //              est_range + "," +
                    //              track.MeasTrueRcycz.TruePos.v[0] + "," +
                    //              track.MeasTrueRcycz.TruePos.v[1] + "," +
                    //              track.MeasTrueRcycz.TruePos.v[2] + "," +
                    //              true_range + "," +
                    //              track.MeasTrueRcycz.R + "," + 
                    //              error_range);

                    sw.Flush();
                }

                sw.Close();
            }
        }
    }
}
